#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from livox_interfaces.msg import CustomMsg
from sensor_msgs.msg import PointCloud2, PointField
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Bool, Int32

import numpy as np
import struct
import open3d as o3d
from threading import Thread, Lock
import time
import multiprocessing

# 定义PointCloud2消息的字段结构，这是一个标准的ROS定义
FIELDS = [
    PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
    PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
    PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
    PointField(name='intensity', offset=12, datatype=PointField.UINT8, count=1), # 注意：优化后的打包函数将使用FLOAT32，但为了兼容性保留定义
]
POINT_STEP = 16 # 优化：将intensity视为float32处理，避免struct打包时的复杂性，x,y,z,i均为4字节

def process_chunk(chunk, distance_threshold, keep_front, area_half_width, area_half_height,
                 exclude_area_params):
    """
    【并行处理单元】过滤点云数据块的函数。
    这个函数被设计为可以被多个进程并行调用，以加速过滤过程。
    """
    # 按距离过滤：计算每个点到原点距离的平方，避免开方运算以提高效率
    squared_distances = np.sum(chunk[:, :3] ** 2, axis=1)
    distance_mask = squared_distances <= (distance_threshold ** 2)

    # 如果设置了只保留前方区域，则增加前方区域的掩码
    if keep_front:
        in_front_mask = (chunk[:, 0] > 0)
        in_width_mask = (np.abs(chunk[:, 1]) <= area_half_width)
        in_height_mask = (np.abs(chunk[:, 2]) <= area_half_height)
        front_area_mask = in_front_mask & in_width_mask & in_height_mask
        filter_mask = distance_mask & front_area_mask
    else:
        filter_mask = distance_mask

    # 创建排除区域的掩码
    in_exclude_area = (
        (chunk[:, 0] >= exclude_area_params['x_min']) &
        (chunk[:, 0] <= exclude_area_params['x_max']) &
        (chunk[:, 1] >= exclude_area_params['y_min']) &
        (chunk[:, 1] <= exclude_area_params['y_max']) &
        (chunk[:, 2] >= exclude_area_params['z_min']) &
        (chunk[:, 2] <= exclude_area_params['z_max'])
    )

    # 合并所有掩码：保留在过滤区域内且不在排除区域内的点
    final_mask = filter_mask & ~in_exclude_area
    return chunk[final_mask]


class LidarProcessor(Node):
    """
    LiDAR点云处理器节点类。
    负责订阅原始LiDAR数据，进行过滤、聚类、目标识别、碰撞检测，并发布处理结果。
    """

    def __init__(self):
        super().__init__('lidar_processor')

        # --- 1. ROS通信设置 (订阅者与发布者) ---
        self._setup_ros_communication()

        # --- 2. 参数配置 (将参数分组管理，提高可读性) ---
        self._load_parameters()

        # --- 3. 状态变量和线程锁初始化 ---
        self.frame_count = 0  # 帧计数器
        self.cluster_history = {} # 用于跟踪聚类的历史信息，以判断其稳定性
        self.point_cloud_lock = Lock() # ✅ 修正：在启动线程前初始化锁

        # --- 4. 可视化设置 ---
        # 根据参数决定是否初始化可视化窗口和相关线程
        self._setup_visualization()

        # --- 5. 并行处理设置 ---
        # 使用CPU核心数减1作为并行处理的进程数，以保留一个核心给系统
        self.num_processes = max(1, multiprocessing.cpu_count() - 1)
        self.get_logger().info(f"节点初始化完成，使用 {self.num_processes} 个进程进行并行处理。")

    def _setup_ros_communication(self):
        """初始化所有ROS订阅者和发布者。"""
        # 声明并获取输入话题参数
        self.declare_parameter('input_cloud', '/livox/lidar_avia')
        input_topic = self.get_parameter('input_cloud').get_parameter_value().string_value

        # 创建订阅者，接收livox原始数据
        self.sub = self.create_subscription(CustomMsg, input_topic, self.callback, 10)

        # 创建发布者，用于发布不同类型的处理结果
        self.pcl_pub = self.create_publisher(PointCloud2, '/livox/pointcloud2', 10) # 处理后的主点云
        self.box_pub = self.create_publisher(PointCloud2, '/detected_cuboid', 10)  # 识别出的目标长方体点云
        self.pose_pub = self.create_publisher(PoseStamped, '/cuboid_pose', 10)      # 目标位姿
        self.collision_pub = self.create_publisher(Bool, '/collision_warning', 10) # 碰撞预警
        self.risk_level_pub = self.create_publisher(Int32, '/collision_risk_level', 10)# 风险等级

        self.get_logger().info(f"订阅: {input_topic}，发布处理后点云到: /livox/pointcloud2")

    def _load_parameters(self):
        """加载并组织所有ROS参数。"""
        # 可视化与保存
        self.declare_parameter('enable_visualization', True)
        self.enable_visualization = self.get_parameter('enable_visualization').get_parameter_value().bool_value
        self.declare_parameter('save_path', "/home/user/pointclouds/") # 优化：将保存路径设为参数
        self.save_path = self.get_parameter('save_path').get_parameter_value().string_value
        self.save_enabled = False # 可通过服务或参数动态修改

        # 点云过滤参数
        self.filtering_params = {
            'distance_threshold': 50.0,
            'keep_front': True,
            'area_width': 2.0,
            'area_height': 2.0,
            'area_half_width': 1.0,
            'area_half_height': 1.0,
            'exclude_area': {
                'x_min': 0.0, 'x_max': 2.8,
                'y_min': -1.0, 'y_max': 1.0,
                'z_min': -2.0, 'z_max': -0.1
            }
        }

        # 聚类分析参数
        self.clustering_params = {
            'enable': True,
            'voxel_size': 0.03,
            'eps': 0.1,
            'min_points': 30,
            'min_visualize_points': 0,
            'max_visualize_points': 8000
        }

        # 长方体识别参数
        self.cuboid_params = {
            'min_length': 0.2, 'max_length': 0.45,
            'min_width': 0.2, 'max_width': 0.45,
            'min_height': 0.2, 'max_height': 0.4,
            'aspect_ratio': 1.5
        }

        # 风险评估参数
        self.risk_params = {
            'threshold_high': 0.15, # 高风险阈值
            'threshold_low': 0.5,   # 低风险阈值
            'levels': {
                0: "无风险", 1: "存在风险", 2: "高风险"
            }
        }

        # 聚类稳定性参数
        self.stability_params = {
            'threshold': 3, # 连续出现3帧认为稳定
            'max_history': 5 # 最多保留最近5帧的历史记录
        }

        # 打印关键参数信息
        self.get_logger().info(f"长方体识别尺寸: L({self.cuboid_params['min_length']}-{self.cuboid_params['max_length']}) W({self.cuboid_params['min_width']}-{self.cuboid_params['max_width']}) H({self.cuboid_params['min_height']}-{self.cuboid_params['max_height']})")
        self.get_logger().info(f"风险分级: 高风险<{self.risk_params['threshold_high']}m, 存在风险<{self.risk_params['threshold_low']}m")
        ep = self.filtering_params['exclude_area']
        self.get_logger().info(f"区域屏蔽: X[{ep['x_min']},{ep['x_max']}], Y[{ep['y_min']},{ep['y_max']}], Z[{ep['z_min']},{ep['z_max']}]")

    def _setup_visualization(self):
        """配置Open3D可视化环境。"""
        if not self.enable_visualization:
            self.get_logger().info("可视化已关闭，将提高处理速度。")
            self.vis = None
            return

        self.pcd = o3d.geometry.PointCloud()
        self.target_pcd = o3d.geometry.PointCloud()
        self.collision_pcd = o3d.geometry.PointCloud()
        self.line_set = o3d.geometry.LineSet()
        self.vis = o3d.visualization.Visualizer()
        self.vis.create_window()
        self.vis.get_render_option().point_size = 2.0
        self.vis.get_render_option().background_color = np.array([0, 0, 0])
        self.is_visualizing = True
        self.is_first_frame_vis = True

        # 启动可视化线程
        self.visualization_thread = Thread(target=self.visualization_loop)
        self.visualization_thread.daemon = True # 设置为守护线程，主程序退出时线程也退出
        self.visualization_thread.start()

    # --- 核心处理流程 ---

    def callback(self, msg: CustomMsg):
        """
        【主回调函数】处理每一帧LiDAR数据，作为整个处理流程的调度器。
        """
        start_time = time.time()

        # 1. 将ROS消息高效转换为Numpy数组
        points_array = self._convert_msg_to_numpy(msg)
        if points_array.size == 0:
            self.get_logger().warn("接收到空点云消息。")
            return

        # 2. 并行过滤点云
        filtered_points = self._filter_points(points_array)

        # 如果过滤后点太少，则跳过后续复杂处理
        if len(filtered_points) < 50:
            if self.frame_count % 20 == 0: # 降低日志频率
                self.get_logger().info(f"过滤后点太少({len(filtered_points)}点)，跳过聚类。")
            # 仍然发布过滤后的点云
            pcl_msg = self._create_pointcloud2_msg(msg.header, filtered_points)
            self.pcl_pub.publish(pcl_msg)
            return

        # 3. 执行聚类分析
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(filtered_points[:, :3])

        downsampled_pcd, labels = self._perform_clustering(pcd)
        if labels is None: # 如果不启用聚类或聚类失败
            colors = np.zeros((len(filtered_points), 3)) # 默认为黑色
        else:
            # 将聚类标签映射回原始高密度点云
            # 注意：此步骤在点云数量巨大时可能成为瓶颈
            full_labels = self._map_labels_to_full_cloud(pcd, downsampled_pcd, labels)

            # 4. 识别目标长方体并分离其他聚类
            target_info, other_clusters = self._identify_target_and_others(filtered_points, full_labels)

            # 5. 进行碰撞检测与风险评估
            if target_info:
                collision_info = self._assess_collision_risk(target_info, other_clusters)
                target_info.update(collision_info) # 将碰撞信息合并到目标信息中
                # <<< 修改：在日志中增加了“倾斜角”的打印 >>>
                self.get_logger().info(f"目标已识别 - 风险: {self.risk_params['levels'][target_info['risk_level']]}, "
                                     f"中心:({target_info['center'][0]:.2f},{target_info['center'][1]:.2f},{target_info['center'][2]:.2f}), "
                                     f"倾斜角: {target_info['tilt_angle']:.2f}°, "
                                     f"最小距离: {target_info['min_distance']:.4f}m")
            else: # 未找到目标
                if self.frame_count % 5 == 0: # 降低日志频率
                    self.get_logger().info("未检测到符合条件的长方体目标。")
                target_info = {'risk_level': 0, 'collision_detected': False, 'collision_clusters': []} # 设置默认值

            # 6. 为点云着色并发布结果
            colors = self._generate_point_colors(filtered_points, full_labels, target_info, other_clusters)
            self._publish_results(msg.header, filtered_points, target_info)

        # 7. 更新可视化数据
        if self.vis:
            self._update_visualization_data(pcd, colors, target_info)

        # --- 帧处理结束 ---
        self.frame_count += 1
        process_time = (time.time() - start_time) * 1000
        self.get_logger().info(f"帧 {self.frame_count}: 原始{len(msg.points)}点 -> 过滤后{len(filtered_points)}点 -> "
                             f"处理耗时{process_time:.2f}ms")

    # --- 私有辅助方法 ---

    def _convert_msg_to_numpy(self, msg: CustomMsg) -> np.ndarray:
        """【优化】高效地将Livox CustomMsg转换为Numpy数组。"""
        # 使用列表推导式，比预分配空间再循环填充更快
        points = [[p.x, p.y, p.z, p.reflectivity] for p in msg.points]
        return np.array(points, dtype=np.float32)

    def _filter_points(self, points_array: np.ndarray) -> np.ndarray:
        """使用多进程并行过滤点云。"""
        # 将点云数据分割成多个块，以便并行处理
        chunks = np.array_split(points_array, self.num_processes)
        # 创建进程池
        with multiprocessing.Pool(processes=self.num_processes) as pool:
            # starmap可以向函数传递多个参数
            results = pool.starmap(
                process_chunk,
                [(chunk, self.filtering_params['distance_threshold'], self.filtering_params['keep_front'],
                  self.filtering_params['area_half_width'], self.filtering_params['area_half_height'],
                  self.filtering_params['exclude_area']) for chunk in chunks]
            )
        # 合并所有处理完的结果
        return np.concatenate(results) if results else np.array([])

    def _perform_clustering(self, pcd: o3d.geometry.PointCloud):
        """对点云进行降采样和DBSCAN聚类。"""
        if not self.clustering_params['enable']:
            return None, None

        # 使用体素网格降采样，减少计算量，同时均匀化点云密度
        downsampled_pcd = pcd.voxel_down_sample(voxel_size=self.clustering_params['voxel_size'])

        # 执行DBSCAN聚类
        labels = np.array(downsampled_pcd.cluster_dbscan(
            eps=self.clustering_params['eps'],
            min_points=self.clustering_params['min_points'],
            print_progress=False
        ))

        return downsampled_pcd, labels

    def _map_labels_to_full_cloud(self, full_pcd, downsampled_pcd, labels):
        """将降采样后点云的聚类标签映射回原始高密度点云。"""
        # 为降采样的点云构建KDTree，用于快速最近邻搜索
        pcd_tree = o3d.geometry.KDTreeFlann(downsampled_pcd)

        # 为原始点云中的每个点找到其在降采样点云中最近的邻居，并赋予其标签
        # 这是一个计算密集型步骤
        full_labels = np.zeros(len(full_pcd.points), dtype=np.int32)
        for i in range(len(full_pcd.points)):
            # search_knn_vector_3d 返回 [k, indices, distances]
            [_, idx, _] = pcd_tree.search_knn_vector_3d(full_pcd.points[i], 1)
            if idx: # 确保找到了点
                full_labels[i] = labels[idx[0]]
            else:
                full_labels[i] = -1 # 未找到则标记为噪声
        return full_labels

    def _identify_target_and_others(self, points: np.ndarray, labels: np.ndarray):
        """从聚类结果中识别目标长方体，并将其余聚类分组。"""
        unique_labels = np.unique(labels)
        cuboid_candidates = []
        other_clusters = []

        for label in unique_labels:
            if label == -1: continue # 跳过噪声点

            cluster_mask = (labels == label)
            cluster_points = points[cluster_mask][:, :3]

            # 过滤掉点数过少或过多的聚类
            if not (self.clustering_params['min_visualize_points'] <= len(cluster_points) <= self.clustering_params['max_visualize_points']):
                continue

            features = self._calculate_cuboid_features(cluster_points)
            if self._is_cuboid(features):
                # 计算聚类中心到车辆前进方向(X轴)的径向距离，用于筛选目标
                distance_to_x_axis = np.sqrt(features['center'][1]**2 + features['center'][2]**2)
                cuboid_candidates.append({
                    'points': cluster_points, 'features': features, 'mask': cluster_mask,
                    'center': features['center'], 'label': label, 'distance_to_x': distance_to_x_axis
                })
            else:
                other_clusters.append({
                    'points': cluster_points, 'center': features['center'], 'mask': cluster_mask, 'label': label
                })

        # <<< 新增日志：打印聚类分析结果 >>>
        if self.frame_count % 2 == 0: # 降低日志频率
             self.get_logger().info(f"聚类分析: 发现 {len(cuboid_candidates)} 个候选长方体, {len(other_clusters)} 个其他聚类。")

        # 如果有候选长方体，选择径向距离最小的作为目标
        if cuboid_candidates:
            cuboid_candidates.sort(key=lambda x: x['distance_to_x'])
            target_cuboid = cuboid_candidates[0]
            # 计算倾斜角
            target_cuboid['tilt_angle'] = self._calculate_tilt_angle(target_cuboid['center'])
            return target_cuboid, other_clusters

        return None, other_clusters

    def _assess_collision_risk(self, target_info: dict, other_clusters: list) -> dict:
        """
        【优化】评估目标与其他聚类之间的碰撞风险。
        使用Open3D高效计算点云间距离，取代随机采样。
        """
        target_pcd = o3d.geometry.PointCloud()
        target_pcd.points = o3d.utility.Vector3dVector(target_info['points'])
        
        # <<< 新增日志：打印目标货物的YOZ平面边界信息 >>>
        tf = target_info['features']
        self.get_logger().info(
            f"目标YOZ边界: Y=[{tf['min_box'][1]:.4f}, {tf['max_box'][1]:.4f}], "
            f"Z=[{tf['min_box'][2]:.4f}, {tf['max_box'][2]:.4f}]"
        )

        collision_clusters = []
        min_overall_distance = float('inf')

        for cluster in other_clusters:
            cluster_pcd = o3d.geometry.PointCloud()
            cluster_pcd.points = o3d.utility.Vector3dVector(cluster['points'])

            cluster_points_np = np.asarray(cluster_pcd.points) # 预先转换为Numpy数组
            cluster_min_x = np.min(cluster_points_np[:, 0])
            target_min_x = target_info['features']['min_box'][0]
            is_higher = cluster_min_x < target_min_x

            self._update_cluster_history(cluster['label'])
            stability_factor = 1.5 if not self._is_cluster_stable(cluster['label']) else 1.0

            current_distance = float('inf')
            dist_type_info = "" # 用于日志记录
            
            if is_higher:
                # 如果聚类更高，主要关注YOZ平面上的距离
                dist_type_info = "(高于目标, 计算YOZ距离)"
                target_points_np = np.asarray(target_pcd.points)
                target_yoz_points = np.c_[np.zeros(len(target_points_np)), target_points_np[:, 1:]]
                cluster_yoz_points = np.c_[np.zeros(len(cluster_points_np)), cluster_points_np[:, 1:]]

                target_yoz_pcd = o3d.geometry.PointCloud()
                target_yoz_pcd.points = o3d.utility.Vector3dVector(target_yoz_points)
                cluster_yoz_pcd = o3d.geometry.PointCloud()
                cluster_yoz_pcd.points = o3d.utility.Vector3dVector(cluster_yoz_points)

                # 计算两个点云在YOZ平面上的最近距离
                dist_matrix = target_yoz_pcd.compute_point_cloud_distance(cluster_yoz_pcd)
                if dist_matrix: current_distance = min(dist_matrix)
            else:
                # 否则，计算三维空间中的最近距离
                dist_type_info = "(低于/平齐, 计算3D距离)"
                dist_matrix = target_pcd.compute_point_cloud_distance(cluster_pcd)
                if dist_matrix: current_distance = min(dist_matrix)
            
            # <<< 新增日志：打印每个聚类的检测详情 >>>
            self.get_logger().info(f"  - 检测聚类 {cluster['label']} {dist_type_info}: 距离 = {current_distance:.4f}m")
            
            min_overall_distance = min(min_overall_distance, current_distance)

            if current_distance < self.risk_params['threshold_low'] * stability_factor:
                collision_clusters.append(cluster)

        # <<< 新增日志：打印全局最小距离 >>>
        self.get_logger().info(f"风险评估完成: 全局最小距离为 {min_overall_distance if min_overall_distance != float('inf') else 'N/A'}m")

        risk_level = 0
        if min_overall_distance < self.risk_params['threshold_high']:
            risk_level = 2
        elif min_overall_distance < self.risk_params['threshold_low']:
            risk_level = 1

        return {
            'collision_detected': bool(collision_clusters),
            'collision_clusters': collision_clusters,
            'risk_level': risk_level,
            'min_distance': min_overall_distance if min_overall_distance != float('inf') else -1.0
        }

    def _generate_point_colors(self, points, labels, target_info, other_clusters):
        """根据聚类和风险评估结果为点云生成颜色。"""
        colors = np.zeros((len(points), 3)) # 默认黑色

        # 为其他聚类着色
        for cluster in other_clusters:
            # <<< 修改：恢复为固定的淡蓝色，方便观察，而不是彩虹色 >>>
            rgb = self._hsv_to_rgb(0.55, 0.8, 0.9) # 使用固定的HSV值生成淡蓝色
            colors[cluster['mask']] = rgb

        # 为有碰撞风险的聚类着色
        if 'collision_clusters' in target_info:
            for cluster in target_info['collision_clusters']:
                if target_info['risk_level'] == 2:
                    colors[cluster['mask']] = [1.0, 0.4, 0.7] # 粉红色 (高风险)
                elif target_info['risk_level'] == 1:
                    colors[cluster['mask']] = [0.2, 0.5, 1.0] # 蓝色 (存在风险)

        # 为目标长方体着色
        if target_info and target_info.get('mask') is not None:
            colors[target_info['mask']] = [1.0, 0.0, 0.0] # 红色

        return colors

    def _publish_results(self, header, points, target_info):
        """发布所有处理结果到对应的ROS话题。"""
        # 发布处理后的主点云
        pcl_msg = self._create_pointcloud2_msg(header, points)
        self.pcl_pub.publish(pcl_msg)

        # 发布目标和风险信息
        if target_info and 'points' in target_info:
            cuboid_msg = self._create_pointcloud2_msg(header, target_info['points'])
            self.box_pub.publish(cuboid_msg)

            pose_msg = self._create_pose_message(header, target_info['center'], target_info['tilt_angle'])
            self.pose_pub.publish(pose_msg)

        # 统一发布碰撞和风险等级消息，即使没有目标
        collision_msg = Bool()
        collision_msg.data = target_info['collision_detected']
        self.collision_pub.publish(collision_msg)

        risk_msg = Int32()
        risk_msg.data = target_info['risk_level']
        self.risk_level_pub.publish(risk_msg)

    def _update_visualization_data(self, pcd, colors, target_info):
        """用最新的数据更新Open3D可视化对象。"""
        with self.point_cloud_lock:
            # 更新主点云
            self.pcd.points = pcd.points
            self.pcd.colors = o3d.utility.Vector3dVector(colors)

            # 更新目标点云
            if target_info and 'points' in target_info:
                self.target_pcd.points = o3d.utility.Vector3dVector(target_info['points'])
                self.target_pcd.colors = o3d.utility.Vector3dVector(np.tile([1.0, 0, 0], (len(target_info['points']), 1)))
            else:
                self.target_pcd.clear()

            # 更新碰撞风险点云
            if 'collision_clusters' in target_info and target_info['collision_clusters']:
                collision_points = np.vstack([c['points'] for c in target_info['collision_clusters']])
                self.collision_pcd.points = o3d.utility.Vector3dVector(collision_points)
                if target_info['risk_level'] == 2: color = [1.0, 0.4, 0.7]
                elif target_info['risk_level'] == 1: color = [0.2, 0.5, 1.0]
                else: color = [0, 1, 0]
                self.collision_pcd.colors = o3d.utility.Vector3dVector(np.tile(color, (len(collision_points), 1)))
            else:
                self.collision_pcd.clear()

            # 更新指向目标的连线
            if target_info and 'center' in target_info:
                self.line_set.points = o3d.utility.Vector3dVector([[0,0,0], target_info['center']])
                self.line_set.lines = o3d.utility.Vector2iVector([[0,1]])
                if target_info['risk_level'] == 2: color = [1.0, 0.4, 0.7]
                elif target_info['risk_level'] == 1: color = [0.2, 0.5, 1.0]
                else: color = [0, 1, 0]
                self.line_set.colors = o3d.utility.Vector3dVector([color])
            else:
                self.line_set.clear()

    # --- 工具函数 ---

    def _calculate_cuboid_features(self, cluster_points):
        """计算聚类的几何特征。"""
        # 使用轴对齐包围盒(AABB)作为特征
        min_box = np.min(cluster_points, axis=0)
        max_box = np.max(cluster_points, axis=0)
        dimensions = max_box - min_box
        length, width, height = sorted(dimensions, reverse=True)
        return {
            "length": length, "width": width, "height": height,
            "volume": np.prod(dimensions),
            "aspect_ratio": length / width if width > 1e-6 else float('inf'),
            "min_box": min_box, "max_box": max_box,
            "center": np.mean(cluster_points, axis=0)
        }

    def _is_cuboid(self, features):
        """根据几何特征判断聚类是否符合长方体定义。"""
        p = self.cuboid_params
        return (
            p['min_length'] <= features["length"] <= p['max_length'] and
            p['min_width'] <= features["width"] <= p['max_width'] and
            p['min_height'] <= features["height"] <= p['max_height'] and
            features["aspect_ratio"] <= p['aspect_ratio']
        )

    def _calculate_tilt_angle(self, center):
        """计算目标中心与原点连线相对于X轴的夹角(度)。"""
        # 点积公式: a · b = |a| |b| cos(theta)
        vector_norm = np.linalg.norm(center)
        if vector_norm == 0: return 0.0
        # X轴单位向量是 [1, 0, 0]，点积结果即为 center[0]
        cos_theta = np.clip(center[0] / vector_norm, -1.0, 1.0)
        return np.degrees(np.arccos(cos_theta))

    def _update_cluster_history(self, cluster_label):
        """更新聚类历史记录，用于稳定性检测。"""
        history = self.cluster_history.get(cluster_label, [])
        history.append(self.frame_count)
        # 只保留最近的记录
        self.cluster_history[cluster_label] = history[-self.stability_params['max_history']:]

    def _is_cluster_stable(self, cluster_label):
        """判断一个聚类是否连续出现，从而判断其是否为稳定障碍物。"""
        history = self.cluster_history.get(cluster_label, [])
        # 如果历史记录足够长，并且帧号是连续的
        if len(history) >= self.stability_params['threshold']:
            return all(history[i] == history[i-1] + 1 for i in range(1, len(history)))
        return False

    def _create_pose_message(self, header, center, tilt_angle):
        """创建位姿消息(PoseStamped)。"""
        pose = PoseStamped()
        pose.header = header
        pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = float(center[0]), float(center[1]), float(center[2])
        # 将倾斜角转换为四元数表示（绕Y轴旋转）
        angle_rad_half = np.radians(tilt_angle) / 2
        pose.pose.orientation.y = np.sin(angle_rad_half)
        pose.pose.orientation.w = np.cos(angle_rad_half)
        return pose

    def _hsv_to_rgb(self, h, s, v):
        """HSV转RGB颜色空间，用于生成多样化的颜色。"""
        if s == 0.0: return v, v, v
        i = int(h * 6.0)
        f = (h * 6.0) - i
        p = v * (1.0 - s)
        q = v * (1.0 - s * f)
        t = v * (1.0 - s * (1.0 - f))
        i %= 6
        if i == 0: return [v, t, p]
        if i == 1: return [q, v, p]
        if i == 2: return [p, v, t]
        if i == 3: return [p, q, v]
        if i == 4: return [t, p, v]
        if i == 5: return [v, p, q]

    def _create_pointcloud2_msg(self, header, points_array):
        """【优化】创建ROS PointCloud2消息，将intensity也作为float处理。"""
        pcl_msg = PointCloud2()
        pcl_msg.header = header
        pcl_msg.height = 1
        pcl_msg.width = len(points_array)
        # PointField的定义中，datatype FLOAT32是7
        pcl_msg.fields = [
            PointField(name='x', offset=0, datatype=7, count=1),
            PointField(name='y', offset=4, datatype=7, count=1),
            PointField(name='z', offset=8, datatype=7, count=1),
            PointField(name='intensity', offset=12, datatype=7, count=1)
        ]
        pcl_msg.is_bigendian = False
        pcl_msg.point_step = POINT_STEP # 16 bytes
        pcl_msg.row_step = pcl_msg.point_step * pcl_msg.width
        pcl_msg.is_dense = True # 假设过滤后没有无效点

        # 将Numpy数组直接转换为字节流，非常高效
        # 如果输入点云只有3列(x,y,z)，则补上一列0作为intensity
        if points_array.shape[1] == 3:
            points_array = np.hstack([points_array, np.zeros((len(points_array), 1), dtype=np.float32)])

        pcl_msg.data = points_array.astype(np.float32).tobytes()
        return pcl_msg

    def visualization_loop(self):
        """可视化线程主循环。"""
        while rclpy.ok() and self.is_visualizing:
            with self.point_cloud_lock:
                if self.is_first_frame_vis and len(self.pcd.points) > 0:
                    self.vis.add_geometry(self.pcd)
                    self.vis.add_geometry(self.target_pcd)
                    self.vis.add_geometry(self.collision_pcd)
                    self.vis.add_geometry(self.line_set)

                    ctr = self.vis.get_view_control()
                    ctr.set_lookat([0, 0, 0])
                    ctr.set_up([0, 1, 0])
                    ctr.set_front([0, 0, 1])
                    ctr.set_zoom(0.5)

                    self.is_first_frame_vis = False
                else:
                    self.vis.update_geometry(self.pcd)
                    self.vis.update_geometry(self.target_pcd)
                    self.vis.update_geometry(self.collision_pcd)
                    self.vis.update_geometry(self.line_set)

            if not self.vis.poll_events():
                self.get_logger().info("可视化窗口已关闭。")
                self.is_visualizing = False
                break
            self.vis.update_renderer()
            time.sleep(0.03)

    def destroy_node(self):
        """节点销毁前的清理工作。"""
        if self.vis:
            self.is_visualizing = False
            # 增加一个微小的延时，确保循环能够检测到 is_visualizing 的变化
            time.sleep(0.05)
            self.visualization_thread.join() # 等待可视化线程结束
            self.vis.destroy_window()
        self.get_logger().info("节点已销毁。")
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = LidarProcessor()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()